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CONFIGURATION CONTROL OF SEVEN DEGREE OF FREEDOM ARMS 

AWARDS ABSTRACT 

A seven-degree-of-freedom robot arm with a six-degree- 
of-freedom end effector is controlled by a processor 
employing a 6-by-7 Jacobian matrix for defining location 
and orientation of the end effector in terms of the 
rotation angles of the joints, a 1 (or more) -by- 7 Jacobian 
matrix for defining 1 (or more) user-specified kinematic 
functions constraining location or movement of selected 
portions of the arm in terms of the joint angles, the 
processor combining the two Jacobian matrices to produce an 
augmented 7 (or more)-by-7 Jacobian matrix, the processor 
effecting control by computing in accordance with forward 
kinematics from the augmented 7-by-7 Jacobian matrix and 
from the seven joint angles of the arm a set of seven 
desired joint angles for transmittal to the joint servo 
loops of the arms. One of the kinematic functions 
constrains the orientation of the elbow plane of the arm. 
Another one of the kinematic functions minimizing a sum of 
gravitational torques on the joints. Still another one of 
the kinematic functions constrains the location of the arm 
to perform collision avoidance. Generically, one of the 
kinematic functions minimizes a sum of selected mechanical 
parameters of at least some of the joints associated with 
weighting coefficients which may be changed during arm 
movement. The mechanical parameters may be velocity errors 
or position errors or gravity torques associated with 
individual joints. 
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CONFIGURATION CONTROL OF 

BACKGROUND OF THE INVENTION 

Technical Field: 

The invention is related to the use of the configuration 
control method disclosed in U.S. Patent No. 4,555,993 by one 
of the inventors herein to the control of seven degree of 
freedom robot arms, using a forward kinematic approach. 

Background Art: 

U.S. Patent No. 4,555,993, the disclosure of which is 
hereby incorporated herein by reference, discloses a 
configuration control method employed in the present 
invention. 
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1. Introduction 

It has been recognized that robot arms with seven or more 
degrees-of-freedom (DOF) offer considerable dexterity and 
versatility over conventional six DOF arms [1]. These 
high-performance robot arms are kinematically redundant since 
they have more than the six joints required for arbitrary 
placement of the end-effector in the three-dimensional 
workspace. Kinematically redundant arms have the potential to 
approach the capabilities of the human arm, which also has 
seven independent joint degrees-of-freedom [2]. 
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Although the availability of the "extra" joints can 
provide dexterous motion of the arm, proper utilization of 
this redundancy poses a challenging and difficult problem. 
Redundant manipulators have an infinite number of joint 
5 motions which lead to the same end-effector trajectory. This 

richness in the choice of joint motions complicates the 
manipulator control problem considerably. Typically, the 
kinematic component of a redundant manipulator control scheme 
must generate a set of joint angle trajectories, from the 
10 infinite set of possible trajectories, which causes the 

end-effector to follow a desired trajectory while satisfying 
additional constraints, such as collision avoidance, 
servomotor torgue minimization, singularity avoidance, or 
joint limit avoidance. Developing techniques to simultaneously 
15 achieve end-effector trajectory control while meeting 

additional task requirements is known as the redundancy 
resolution/ problem, since the motion of the manipulator 
joints must be "resolved" to satisfy both objectives. 

20 Since redundancy is an important evolutionary step toward 

versatile manipulation, research activity in redundancy 
resolution and related areas has grown considerably in recent 
years, [e.g. 3-10] . For the most part, researchers have been 
working with a set of analytical tools based on linearized 
25 differential/ kinematics models. Previous investigations of 

redundant manipulators have often focused on local/ 
optimization for redundancy resolution by using the Jacobian 
pseudoinverse to solve the instantaneous relationship between 
the joint and end-effector velocities. Redundancy resolution 
30 based on the Jacobian pseudoinverse was first proposed by 

Whitney [3] in 1969, and the null-space projection improvement 
was proposed by Liegeois [4] in 1977. Over the past two 
decades, most researchers have continued to develop variations 
of the pseudoinverse approach primarily because the complex 
nonlinear forward and inverse kinematics models have deterred 
further investigations into new redundancy resolution schemes. 
A conceptually simple approach to control of redundant 
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manipulator configuration has been developed recently based on 
augmentation of the manipulator forward kinematics [11]. This 
approach covers a wide range of applications and enables a 
major advancement in both understanding and developing new 
5 redundancy resolution methods. This paper presents the 

applications of the configuration control approach to a large 
class of redundant industrial robot arms with seven 
degrees-of-freedom. 

10 The paper is organized as follows. Section 2 describes 

the kinematics of the 7 DOF Robotics Research arm and gives an 
overview of the configuration control approach. Various 
applications of the configuration control approach to the 7 
DOF arm providing elbow control, collision avoidance, and 
15 optimal joint movement are given in Section 3. Section 4 

describes the laboratory setup and the implementation of 
configuration control for real-time motion control of the 7 
DOF arm, with elbow positioning for redundancy resolution. 
Conclusions drawn from this work are given in Section 5. 

20 

2. Motion Control of 7 DOF Arms 

In this section, we describe the kinematics of the 7 DOF 
Robotics Research arm under study and discuss the motion 
control of this arm using the configuration control approach. 
25 

2 . 1 Kinematics of 7 DOF Robotics Research Arm 

The Robotics Research (RR) arm is one of the few 
kinematically-redundant manipulators that is commercially 
available at the present time [12]. The Model K1207 RR arm has 
30 been purchased by JPL and similar models by other NASA centers 

for research and development of technologies applicable to the 
NASA Space Telerobotics Projects. 

The Robotics Research arm has an anthropomorphic design 
35 with seven revolute joints, as shown in Figure 1 and has 

nonzero offsets at all the joints. The arm is composed of a 
number of "modules" with roll and pitch motions. The shoulder 
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joint with roll and pitch motions moves the upper-arm; the 
elbow joint with roll and pitch actions drives the forearm; 
and the wrist roll and pitch rotations together with the 
tool-plate roll move the hand. Essentially, the 7 DOF arm is 
obtained by adding the upper-arm roll as the 7th joint to a 
conventional 6 DOF arm design. The RR arm is supported by a 
pedestal at the base. 


For kinematic analysis of the RR arm, coordinate frames 
are assigned to the links in such a way that the joint 
rotation 0, is about the coordinate axis z 4 and the base frame 

fro’Vo* -*- s a ttached to the pedestal. The two consecutive frames 


H-l’^- 1 , 2 . with origin 0,., and with origin 0 ; are 

related by the 4x4 homogeneous transformation matrix [13] 


20 


cosOj 

-SIMd 1 0 


SINQ i COSa i _ 1 

COSQ 1 COSa i _ 1 -SINa i _ 1 

-SINa 

SINtheta i SINa i . 1 

COSQ^INa^ COSa i . 1 

COSa i 

0 

0 0 

1 


( 1 ) 


where d (/ a { , and oij are the link length, joint offset and 
twist angle respectively, given in Table 1. The transformation 
that relates the hand frame {7} to the base frame { O } is 
obtained as 


30 


°t 7 =°t 1 - 1 t 2 
( R 


• 2 c 3 - 3 c 4 * 4 c 5 * 5 c 6 - 6 c 7 
; p\ 


\0 o o ; l) 


( 2 ) 


where R— {r~} is the 3 x 3 hand rotation matrix and p=[x, y, z] T 
is the 3xi hand position vector with respect to the base. One 



7 


common representation of the hand orientation is the triple 
roll-pitch-yaw Euler angles (p, (3,y )• This three-parameter 
representation of hand orientation is subtracted from the hand 
rotation matrix R as follows [13]: 

p=Atan2(r 32 , r 33 ) 

5 p =Atan2(-r 31 , (3) 

Y=Atan2(r 21> r 31 ) 

where Atan2 is the two-argument arc tangent function, and it 
is assumed that the pitch angle /3 is not equal to or greater 
than ± 90°. Therefore, the hand position and orientation can 
be described by the 6xi vector Y=[x, y, z, p, (3, y] T the 
10 three-dimensional workspace. 

The 6x7 Jacobian matrix J v relates the 6xi hand 
rotational and 

15 


translational velocity vector 



to the 7xi joint angular 


20 


velocity vector 6 as V=J v 0. The hand Jacobian matrix is 
computed using the vector cross-product form [14] 



xP 1 2xP 2 - 2 1 xP 1 


\ 

J 


( 4 ) 


where z i is the unit vector along the z -axis of link frame 

{*}, and P' is the position vector from the origin O, of link 

frame {*'} to the origin of hand frame {7}. The Jacobian matrix 
in (4) can be 
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partitioned as 




J,=l 


\ J vt/ 


where J vr and J vt designate the 


5 


10 


rotational and translational components of the Jacobian, that 
is, w=J VT $ and v=J vt 6. In order to relate the joint velocities 

to the rate of change of the roll-pitch-yaw angles that 
represent the hand orientation, the rotational Jacobian J vr in 
(4) is modified to yield [13] 


_d 

dt 


( p ) 

'0 

-SINy 

COSyCOS^ 

p - 

0 

COSy 

SINyCOSfl 

kY> 

^0 

0 

-SIN$ j 


jv r e=nj vr 0 


(5) 


15 


where the transformation matrix II (5) maps w to 


(P) 

0 

\.y j 


and 


det[II] = -C05P *0 since P*±90° 


From (4) and (5), we obtain the 6x7 hand Jacobian matrix 

KM 
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which relates F to # as Y=j e (0)O. 


It is important to note that 
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10 


15 


20 


25 


30 


the computational efficiency can be creased significantly by 
exploiting the commonality of terms between the hand 
transformation matrix °T 7 and the hand Jacobian matrix J . 

Since the Robotics Research arm has seven joints, it 
offers one extra degree of joint redundancy for the task of 
controlling the six hand coordinates. The resolution of this 
single degree-of-redundancy is the subject of the next 
section. 

SUMMARY OF THE INVENTION 

A seven-degree-of-freedom robot arm with a six-degree-of- 
f^®®dom end effector is controlled by a processor employing a 
6— by-7 Jacobian matrix for defining location and orientation 
of the end effector in terms of the rotation angles of the 
joints, a l (or more) -by-7 Jacobian matrix for defining 1 (or 
more) user-specified kinematic functions constraining location 
or movement of selected portions of the arm in terms of the 
joint angles, the processor combining the two Jacobian 

to produce an augmented 7 (or more) —by— 7 Jacobian 
matrix, the processor effecting control by computing in 
accordance with forward kinematics from the augmented 7-by-7 
Jacobian matrix and from the seven joint angles of the arm a 
set of seven desired joint angles for transmittal to the joint 
servo loops of the arms. One of the kinematic functions 
constrains the orientation of the elbow plane of the arm. 
Another one of the kinematic functions minimizing a sum of 
gravitational torques on the joints. Still another one of the 
kinematic functions constrains the location of the arm to 
perform collision avoidance. Generically, one of the 
kinematic functions minimizes a sum of selected mechanical 
parameters of at least some of the joints associated with 
wsicrh^ing coefficients which may be changed during arm 
movement. The mechanical parameters may be velocity errors or 
position errors or gravity torques associated with individual 
joints. 
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BRIEF DESCRIPTION OF THE DRAWINGS 

Fig. 1 is a perspective view of a seven degree of freedom 
robot arm of the type controlled in the present invention. 

Fig. 2 is a block diagram of an architecture embodying 
the present invention. 

Fig. 3 is a diagram of the robot arm of Fig. 1 in one 
position of interest. 

Fig.'s 4a, 4b, 4c and 4d are diagrams of the robot arm of 
Fig. 1 in various positions of interest. 

Fig. 5 is a diagram illustrating the coordinates employed 
in the detailed description of the invention below. 

Fig. 6 is a graph illustrating the arm angle as a 
function of the number of sampling steps in one implementation 
of the invention. 

Fig.'s 7a and 7b are graphs illustrating joint angles of 
respective joints of the arm of Fig. 1 as a function of the 
number of sampling steps in an implementation of the 
invention. 

Fig.'s 8a and 8b are graphs illustrating a collision 
weighting factor and a collision avoidance critical distance, 
respectively, as a function of the number of sampling steps in 
an implementation of the invention. 

Fig. 9 is a graph illustrating the variation of the arm 
angle as a function of the number of sampling steps in an 
implementation of the invention. 

Fig. 10 is a graph illustrating various joint angles as a 
function of the number of sampling steps in an implementation 
of the invention. 
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Fig. 11 is a block diagram of a hardware system employed 
in carrying out one embodiment of the present invention. 

Fig.'s 12a through 12g are graphs illustrating errors in 
5 respective parameters of joint position and joint angle as a 

function of time in an implementation of the invention. 

DETAILED DESCRIPTION OF THE INVENTION 
10 2.2 Configuration Control of the 7 DOF Arm 

The configuration control approach introduced in [11] is 
a viable technique for resolution of redundancy and motion 
control of redundant manipulators. This approach is based on 
redundancy resolution at the position (i.e., task) level 
through augmentation of the manipulator forward kinematics by 
a set of user-defined kinematic functions 

(0) = (4>i (0) , - , <j> r (0) } , where r is the number of redundant 

manipulator joints. This is contrast to the conventional 
Jacobian pseudoinverse methods which resolve the redundancy at 
the velocity (i.e., differential kinematics) level. 

For the 7 DOF Robotics Research arm, the six hand 
position and orientation coordinates obtained in Section 2 . 1 
are augmented by the scaler user-defined kinematic function <p 
to yield the 7xi configuration vector X=[Y T ,0] T . The redundancy 
resolution goal is then expressed as the additional task 
constraint 

^(0)=^<f(*)( 6 ) 

that will be accomplished simultaneously with the basic task 
of controlling the hand motion Y(0)=Y d (t) , where 0 d (t) and 
Ydft) are the desired time variations of <p(6) and Y (6) 
respectively. Since the functional forms of the kinematic 
function and its desired time evolution are at the user's 
discretion, this approach can accommodate a wide range of 
redundancy resolution goals such as arm posture control (i.e. 
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elbow positioning [15]), satisfaction of a task constraint 
(e.g. collision avoidance [16]), or optimization of a 
kinematic performance measure (e.g. minimal joint movement 
[17]). This formulation puts the redundancy resolution on the 
same footing as the end-effector task, and treats them equally 
within a common format. As a consequence, configuration 
control schemes ensure cyclicity (i.e., conservativeness) of 
arm motion, in contrast to pseudoinverse-based methods. 


The configuration control approach can be implemented 
either as a dynamic or a kinematic control law. In the dynamic 
control implementation [11], the configuration controller 
produces the appropriate joint torques r(t) using a 
joint-space or a task-space formulation. In the kinematic 
control implementation [17], the controller generates the 
appropriate joint angle trajectories 0 d (t) which are then used 
as setpoints for the low-level joint servo-loops. In this 
paper, we adopt the kinematic configuration control approach 

ease of implementation. Since the Robotics Research arm 
has non-zero joint offsets, there are no closed-form 
analytical inverse kinematic solutions and therefore a 
differential kinematics approach must be adopted. The 

augmented differential kinematics model of the arm is obtained 
as 


X(t) = 




v^ c (0), 


6(t) =,7(6)6 ( t) 


(7) 


30 


where J e (0) is the 6x7 hand Jacobian matrix obtained Section 

d(f> 

2.1, J c {0)= — is the 1x7 Jacobian matrix associated with the 


kinematic function <p , and J (0) is the 7x7 augmented Jacobian 
matrix*Note that when <f>(0) is defined as the gradient of an 
objective function to be optimized, J becomes the "extended” 
Jacobian proposed by Baillieul [9] for redundancy resolution. 
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Therefore, the extended Jacobian method is retrieved as a 
special case of the configuration control approach when the 
latter is implemented as a differential kinematic controller 
with an optimization additional task. Assuming det=0, 

5 equation (7) is solved in discrete-time as 


A8 d (A0 =J" 1 (0 J1 ) U d Um) -X(N) ] v 

10 

where N is the sampling instant, 0 and X are the actual values 
while 0 d and X d are the desired values. Note that the use of X 
in (8) corrects for linearization errors due to differential 
kinematics. The next desired joint angle is then computed from 
15 0 d (N+l) = 6 d (N) + AQ 6 (N) , and is sent as a setpoint to the joint 

servo-loops for tracking. 

The configuration control framework allows the user to 
specify multiple additional tasks to be accomplished 
20 simultaneously with the basic task of hand motion. Suppose 

that r(>l) additional task constraints are defined as 
0, ( 6) =0 dl - (t) , i=l,-,r. Then, the augmented differential 

kinematics model becomes 

25 


30 


35 

The optimal (i.e. damped least-squares) solution of the 
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over-determined set of equations (9) that has the smallest 
joint velocity |6|| is given by [17-19] as 


5 


6 = [j t wj+wJ ~ ljT w *d 


( 10 ) 


or in discrete-time implementation 


10 


A6 d (N) = [J t (0 N ) W J(Q n ) + tf v ] - 1 J T (d N ) W[X d (N+\) -X(N) } 


(ID 


where IV=diag{JV e , W c } and W v are the (6+r)x (6+r) and 7x7 

15 matrices of task error and joint velocity weighting factors 

specified by the user. Note that when W v =0, r=l and 

det [<J] *0 , equation (8) is retrieved from (10). The acquired 
solution 6 (10) minimizes the scalar cost function 
20 L=E e T W e E e * E c wj! c * e^e 

where E e =Y^-J e 6 and E c =^^-J c 6 are the basic task and additional 

task velocity errors. The task weighting factors W e , W c enable 
the user to assign priorities to the different basic and 
25 additional task requirements. The joint velocity weighting 

factor W v allows the user to suppress large joint velocities 
near singularities, at the expense of small task errors. This 
is particularly important in redundant arm control because the 
complicated nature of the augmented Jacobian singularities 
deters any analytical characterization of the singular 
configurations . 
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An architecture corresponding to Equations (8) and (10) 
is illustrated in Fig. 2. 


The ability to change the weighting factors on-line based 
on the task performance provides a general framework for 
incorporation of multiple constraints in redundant arm 
control. Equation (10) can be written as 


10 


15 


20 


25 


r 

-1 

i 

K + Je T " e J e + E 







where J cj is the Jacobian related to <j>.. Equation (13) shows 
the contribution of each additional task constraint to the 
optimal joint motion. This formulation can be used to "blend" 
multiple additional tasks or to "switch" between different 
additional tasks by proper selection of their weighting 
factors. For instance, for the 7 DOF arm, we can switch 
between elbow control and collision avoidance during task 
execution so that when the arm is far from workspace 
obstacles, w el =l and w co =0 and direct elbow control will take 
P r ® c ® < ^®nce • A s soon as potential collision is detected (from 
world model or sensory data) , the collision avoidance goal 
becomes dominant and the corresponding weighting factor w co 
creases as the arm gets closer to the obstacle, at the expense 
of loss of direct elbow control by setting w el =0. This feature 

is illustrated in Section 3, and is discussed in detail in 
(17] . 


The configuration control formulation can be used to meet 
diverse additional task constraints for redundancy resolution 
[20]. For instance, the redundancy can be used to control 
^ geometrical variable (such as coordinates of a 
on the arm) , a physical variable (e.g. a joint gravity 
torque) , or a mathematical function (such as projected 
gradient of an optimization function) . In the next section, we 
demonstrate three applications of configuration control for 
motion control of the 7 DOF Robotics Research arm. In each 
application, the single degree-of-redundancy is utilized to 
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accomplish a different additional task objective; namely, 
elbow control, collision avoidance, and optimal joint 
movement . 


5 


3. Graphics Simulation of 7 DOF Arm Control 

This section describes some of the simulations of the 
configuration control scheme for redundancy resolution and 
10 kinematic motion control of the Model K1207 7 DOF Robotics 

Research arm. 

The Silicon Graphics IRIS 4D70-GT is a Workstation with 
both high-speed computing and graphics capabilities, and is 
15 used in this simulation study. A three-dimensional color 

rendering of the Robotics Research K1207 arm is built with a 
set of primitives that use the IRIS "C" language graphics 
library. When the program is run, it initially displays the 
arm and its state information on the IRIS screen as shown in 
20 Figure 3. The rendering of the arm is centered on the screen 

with the joint angles, Cartesian hand coordinates, arm angle, 
manipulability indices, and trajectory time information 
displayed in a table in the lower left corner, the redundancy 
control mode is displayed in the upper left, and the user menu 
25 box (not shown) appears as needed in the upper right corner of 

the screen. Since the zero configuration of this particular 
arm is a singular configuration, the arm shown in this figure 
is in the user-defined "home" configuration. Simulation 
software is written in "C" and animates the kinematic control 
30 results as they are computed so as to move the arm 

continuously on the screen. Figures 4(a) — (d) show the 
evolution of the arm as it moves from an initial to a 
user-specified final configuration. The control law is 
computed and used to continuously change the arm configuration 
and the state information in the lower left corner of the 
screen is updated at every sampling instant. A simple 
cycloidal trajectory generator provides point-to-point 
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straight-line Cartesian paths based on Cartesian goal points 
input by the user either from the keyboard or from the mouse. 
Alternatively, the user may use the mouse in teleoperation 
mode to directly control the arm in joint or Cartesian space, 

5 activating different degrees-of -freedom with the mouse 

buttons. Using a simple stacking feature, the user may save a 
sequence of intermediate points to a file for a later run. The 
user can also select from a number of redundancy resolution 
schemes for each task, adjust optimization parameters or 
10 obstacle location, plot the results of each run, or save the 

data for later analysis. The user may also rerun the 
simulation program, adjusting his viewing location and 
perspective on each run. 

This interactive graphics simulation environment serves 
as an essential tool for development and validation of new 
control schemes for redundant 7 DOF arms. The IRIS also allows 
the user to simulate the robot workspace graphically and plan 
the task sequence. It can then be used for "task preview" by 
simulating the robot control algorithms and animating the task 
scenario. In this mode of operation, the IRIS can be used for 
operator training and rehearsal, prior to actual task 
execution. This preview mode is important in dealing with 
redundant arms, since it enables the user to explore various 
alternatives for redundancy resolution and can reveal 
unexpected behavior of the robot. 

Several configuration control schemes for the 7 DOF 
Robotics Research arm have been designed and verified by 
30 simulation on the IRIS. The case studies presented here are 

samples selected from an extensive computer simulation study 
which was carried out to test the performance of the proposed 
control schemes. These cases are chosen for presentation 
because they illustrate the flexibility and versatility of the 
35 configuration control approach to redundant manipulators. 

Three case studies are presented in this section, namely: 
elbow control, collision avoidance, and optimal joint 
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movement . 
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3 . 1 Elbow Control 

The presence of a redundant joint in the 7 DOF Robotics 
Research arm results in infinite distinct arm configurations 
with the same/ hand position and orientation. This leads to a 
physical phenomenon known as "self-motion" or "orbiting," 
which is a continuous movement of the joints that leaves the 
hand motionless. The self-motion of the RR arm corresponds to 
the elbow point E traversing a circle around the line SW 
joining the shoulder S to the hand W, without moving the hand 
frame. Thus the elbow position, together with the hand 
coordinates, forms a complete representation of the 
geometrical posture (i.e., the physical shape) of the whole 
arm in the entire workspace. One natural representation of the 
elbow position is the "arm angle" $ defined as the angle 
between the arm plane SEW and a reference plane, such as the 
vertical plane passing through the line SW, [15], as depicted 
in Figure 5. The angle i| ; succinctly characterizes the 
self “motion of the arm and uniquely specifies the elbow 
position for a given hand frame. Other viable representations 
of the elbow position are the x, y, or z Cartesian coordinates 
of the elbow (i.e., E x , E y , or E z ) in the base frame. The 
choice of f or a particular elbow coordinate is clearly 
dictated by the task that the arm is required to perform. In a 
recent paper [15], simple and computationally efficient 
methods of computing the arm angle and the associated 

constraint Jacobian are given, where \ff=J^6. Following , i|r 
and are computed from 


i|/=Atan2[w r ( Vxp) , V T p ] 


(14) 
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where E and W are the Jacobian matrices related to the elbow 
and the wrist linear velocities and other symbols are defined 
in Figure 5, with 'caret 7 designating a unit vector. 

5 The user interacts with the IRIS Workstation by using the 

keyboard to enter the desired target position and orientation 
of the hand (x f ,y f , z f , p f/ 0 f , y f ) and the desired final arm 
angle \|r f , as well as the duration of motion t and the sampling 
period At. The hand frame can alternatively be input using the 
10 mouse which essentially emulates a 6 DOF cursor. The 

trajectory generator software then computes smooth cycloidal 
trajectories for these seven variables to change them from 

initial values (Xg, yg, Zg, Pg, ^?g, yg, i/r^) to the final 
values in the specified time duration. For instance, a typical 
15 cycloidal trajectory for the desired arm angle i|r d is 
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Note that only the ratio of elapsed time to motion 
time — is needed for the trajectory generator. In 


25 


discrete— time implementation, the number of samples during 
motion is equal to . Note that, using the cycloidal 


functions, the hand moves on a straight-line path; since we 

x-Xq _ y-y 0 _ 


obtain 


x r x o y f -y 0 z f -z Q 


In this simulation study, the Robotics Research arm is 
initially at the joint configuration 


0(0) = [-90°, -43.3°, o°, -101°, -180°, -54. 3°, -90° ] T 


30 


This yields 
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the initial hand configuration 

P 0 = {x=0 , y=90 , z=0 , p = -90° , P = 0° , y=0° } relative to the base 

frame and the initial arm angle i|r 0 =0° • where the position 

coordinates are in centimeters and the angles are in degrees. 

5 The hand is commanded to trace a triangle by making the 

successive moves: P Q ^ P 1 , P 1 ^P 2 , P 2 X J P o > where 

P x ={ 50,50,50, 0°, 0°, 0°} , i|r 1 = -90° , x x =2 . 0 
P 2 ={-50, 50, 50, 0°, 0°, 0°} , i|r 2 =+45° , t 2 =4.0, t 3 =1.0 

while At=0.025 in all cases and the unit of time is the 

second. The kinematic configuration control scheme is used to 
compute the required joint motions that result in the 

10 commanded hand and arm angle trajectories. Note that 

from (15) is used in (10) , and we set W e =I 6 , W c =l and W v =0 
since no arm singularities are encountered during the motion. 
Figure 6 shows the executed motion of the elbow, in which the 
arm angle changes from 0° to -90° and then to +45° during the 
15 hand motion. The variations of the joint angles ..., 0 7 to 

achieve the commanded arm motion are shown in Figures 7a — 7b. 
These figures illustrate that all the seven joint angles 
return to their initial values at completion of the task. 

Thus, the initial and final arm configurations are identical 
20 and the robot has executed a cyclic (i.e., conservative) 

motion under configuration control. 


25 3.2 Collision Avoidance 

One of the advantages of the 7 DOF arm is the potential 
to use the "extra” DOF to maneuver in a congested workspace 
and avoid collision with obstacles by configuring the arm 
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appropriately without perturbing the hand trajectory. In this 
formulation, all workspace obstacles are enclosed in convex 
volumes and each volume defines a "space of influence" (SOI) 
for the control law. In this study, the SOIs are assumed to be 
5 spheres in the three-dimensional workspace, but extension to 

other geometrical shapes is possible using distance functions 
[21], In the configuration control framework, the collision 
avoidance requirement is formulated as a kinematic equality/ 
constraint 

10 


4> (0) ^ c? c (0) - r 0 z 0 


(17) 


where d c (0) - \\X C ( 0 ) -X 0 1 | is the critical distance between the 

arm and the obstacle, X 0 is the position of the SOI center, r 0 
is the radius of the SOI, and X c is the position of the 
"critical point" on the arm currently at minimum distance from 
the obstacle. Note that the location of the critical point X c 
and the critical distance d c are both configuration dependent 
and are continuously recomputed as described [16]. Two modes 
of operation are possible: 

Case One d c (0)> r 0 : In this case, the equality constraint 
(17) is satisfied and the entire arm is outside the obstacle 
SOI. Therefore, the constraint is active/ and the manipulator 
redundancy can be used to achieve other additional tasks, such 
as those in Sections 3.1 and 3.3. 


Case Two d c (#)<r 0 : In this case, the equality constraint 
(17) is active/ and the arm is inside the obstacle SOI. Thus, 
the redundancy is utilized to avoid collision with the 
obstacle by inhibiting the motion of the critical point on the 
arm in the direction toward the obstacle. To this end, (17) is 
replaced by the equality constraint d c (0) =r 0 , and the 

constraint Jacobian is obtained as J c (fl)= 

' O/l *0/1 
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10 


15 


configuration control scheme can now be employed to achieve 
the desired hand motion as well as collision avoidance. 
However, in this formulation, the additional task constraint 
is either "on" or "off." This can lead to an undesirable rapid 
switching between the "on" and "off" conditions thus resulting 
a "chattering" phenomenon on the SOI boundary. Furthermore, 
switching between the collision avoidance task in Case Two and 
another additional task (such as elbow control) in Case One 
can cause discontinuity problems. The variable task weighting 
scheme alleviates both the chattering and discontinuity 
problems. In this scheme, the weighting factors w el and w co for 
the elbow control and collision avoidance tasks Cases One and 
Two are chosen as functions of the critical distance d c (0), 
instead of predefined constants. The use of variable weighting 
factors for the additional tasks allows the collision 
avoidance constraint to be incorporated gradually with the 
basic task, and furthermore circumvents the discontinuity 
problem in switching between different additional tasks. 
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In the simulation study, the Robotics Research arm is 
initially at the joint configuration 0(O)=[-9O°, -44.7°, 0°, 
-89.4 , 0 , -135.8°, 90°] t . The task is to move the hand on a 
straight-line from the initial location P 0 =[O, 90,0, 0°, 0°, 

90° ] to the target location P^f-90, 30, -30, 0°, 0°, 90°] 
T= 8.75 seconds with At=0.025 such that during motion the arm 
avoids collision with a workspace obstacle. The obstacle is 
enclosed by two SOIs: an inner SOI which touches the actual 
obstacle boundary, and an outer SOI which allows for some 
"buffer." The inner and outer SOIs are concentric spheres with 
centers at z 0 =13.3] and radii rj=8.5 cm and r 0 =37.5 cm. Each 
hand coordinate is required to track a cycloidal trajectory as 
described in Section 3.1. Initially, before the obstacle is 
encountered, it is required to keep the arm angle constant at 
its initial value of iji=0° to resolve the redundancy. When the 
obstacle is encountered, the redundancy is used for collision 
avoidance at the expense of loss of elbow control. After the 
obstacle encounter, the arm angle should remain constant. In 
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this simulation, when the arm is outside the outer SOI (Case 
One) , we set w e( =l and w co =0 to achieve elbow control. As soon 
as potential collision is detected (Case Two) , the redundancy 
resolution goal switches smoothly to collision avoidance by 
setting w el =0 and increasing w co as an inverse square function 
of d c ( 6 ) , that is 


w C o= 


10 10 

Me"/ Mo"/ 


(18) 


for d c <r 0 . Using (18), when the arm is at the outer SOI 
boundary (d c =r 0 ) , we have w co =0 ; and as the arm moves closer 
10 to the obstacle, w co creases rapidly so that w co "<» as d c . 

The variations of w c0 and the critical distance d c (0) are shown 
in Figures 8a-8b. It is seen that the increase in w co has 
hindered motion of the arm inside the inner SOI, thus ensuring 
that collision avoidance is successfully accomplished 
15 throughout the arm motion. The variation of the arm angle <ji(0) 

is shown in Figure 9, and illustrates that the arm angle is 
held constant when the obstacle is not encountered, as 
expected . 


20 3.3 Optimal Joint Movement 

In this case study, the redundancy resolution goal is to 
distribute the hand motion among the joints in such a way that 
a weighted sum of joint movements is kept at minimum. Toward 
this end, the optimization objective function is selected as 

7 

25 G{0)= Y^ 05k xW- e i^ 2 < 19 > 

t=l 

where k, is the weighting factor for joint i movement and 
[0 i (t) -0^(0)] 2 denotes the current deviation of joint angle 

0,-(t) from its initial position 0j(O). The objective function 
G(0) (19) represents the total instantaneous potential energy 

30 stored in seven hypothetical springs attached to the robot 
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joints with stiffness coefficients { k £ and natural lengths 

W/O)}. By choosing appropriate numerical values for {kj , the 

user can resolve the hand motion among the joints such that 
the joints with larger k move less at the expense of those 
5 with smaller k. The ability to penalize individual joint 

movement may also lead to a desirable distribution of joint 
torques for a given hand trajectory [17]. The condition for 
optimality of G(0) subject to the end-effector constraint 

Y=Y(0) has been found [17] to be N e (0)^^= 0, where N is a 1x7 

d6 e 

0 vector which lies in the null-space of the hand Jacobian J e , 

that is J e N e T =0. This implies that for optimality, the 
projection of the gradient of the objective function onto the 
null-space of the hand Jacobian must be zero. To achieve 
optimal joint movement, the kinematic function is defined as 

5 <f>(0)=N e (0) and its desired value is set to 0 d (t)=O, to 

represent the optimality condition. The configuration control 
approach can then be applied to obtain the joint trajectories 
which cause the hand to attain the commanded motion with an 
optimal total joint spring energy. 

) 

In this simulation study, the arm is initially at 
6(0) =[-89 . 1° , -32.1°, -45°, -91.5°, -47°, -126.6°, 29.7°] T 
giving the initial hand coordinates as P 0 =[50, 70, 30, 0°, 0°, 

90 ] . The hand is commanded to move on a straight-line to the 
target location P^f-50, 70, -30, 90°, 0°, 0°] r=2 . 5 seconds 

with At=0.025, while the arm redundancy is used to achieve the 
hand trajectory with optimal joint movement. The user types in 
the stiffness coefficients of the joint springs 

{^ J .}={20,1,1,1,1,1)1}/ where a large value for k 1 dictates the heavy 
penalty on joint 1 movement. The program then computes 
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and J c =& 


d6 


and augments the hand Jacobian J B to 
do e 
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20 


obtain J. The required joint trajectories are then found by 
using (10) with W e =I 6 , W c =l, W v =0. The variations of the joint 
angles are given in Figure 10, which shows that the first 
joint with a large weighting factor has moved considerably 
less than the other joints, as desired. 

3*4 Alternative Redundancy Resolution Goals 

In addition to the redundancy resolution goals discussed 
in Section 3.1 — 3.3, the user can select other criteria from a 
menu presented to him on the IRIS screen. This menu of 
redundancy resolution options is an area of current research, 
and analytical techniques that are being developed are added 
to the menu for test and validation. In this section, we shall 
present some of the items on the redundancy resolution menu. 

(i) Joint Locking: The user can select a particular joint, say 
0j, to be locked during the commanded hand motion. In this 
case, the relationship (t) = 6 } (0) is treated as the additional 
task, with J c =[0, ..., l,...0] . The configuration control approach 
then attempts to move the hand using the remaining six joints 
while keeping Qj constant. This is equivalent to deleting the 
j th column of J e to obtain the 6x6 matrix 

J e and then solving Y=J e 8 for the remaining six joints 6. The 
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acquired solution for 6 depends on the locked value of 0 j; 

namely 0j(O). Note that for some selections of 6 jf the 
resulting Jacobian J e is always singular, which implies that 

from a physical point of view, the hand position and 
orientation can not be changed arbitrarily while 6- is locked. 
The joint locking option is useful in investigating the 
f a i-l“t°l erance feature of the robot joints, i.e., preservation 
hand motion despite a joint failure. In addition, this 
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option can be exercised when the operator only wishes to 
perform the basic task of hand placement and orientation. 

(ii) Joint Limit Avoidance: The joints of any robot have 

5 rotational limitations that can typically be expressed as c*j< 

6-< /Jj, where a- and /3j are the lower and upper joint limits. 
One of the applications of redundancy is to resolve the hand 
motion among the joints such that their limits are not 
violated. The joint limit equality constraint is treated 
10 within the configuration control framework in a similar manner 

to the obstacle avoidance constraint in Section 5.2. The user 
can select the joint limits and command hand motion, and 
examine the robot performance. Since inequality constraints 
are treated as equality conditions for redundancy resolution, 
15 for some joint angles the augmented Jacobian can be singular 

and the problem may not have a solution. 

(iii) Manipulability Maximization: A common objective function 
to be maximized by the utilization of redundancy is the hand 

20 manipulability index [15] defined as /x(^)=‘Jdet. This scalar 

index vanishes at the hand singular configurations where J e ( 6) 
is rank-deficient. Therefore, maximizing n(d) during a 
prescribed hand motion leads to arm configurations which avoid 
the hand Jacobian singularities as much as possible. This 
25 solution can be obtained by following Section 5.3 with G(d) 

replaced by n(d ) . Note that in this case ^ must be computed 

d6 

numerically. 

4. Real-Time Control of the 7 DOF Arm 

30 In this section, we describe the implementation and 

experimental validation of the configuration control scheme on 
the 7 DOF Robotics Research arm. The laboratory setup is 
described first, followed by a description of a simple 
experiment. In this experiment, the configuration control 
approach is implemented for real-time control of the Robotics 
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Research arm, with elbow positioning for redundancy 
resolution. 

The Robotics Research Laboratory at JPL consists of one 
Model K1207 7 DOF arm and control unit from the Robotics 
Research Corporation, a VME-based chassis with MC 68020 
processor boards, two 3 DOF industrial rate joysticks, a 
motorized lathe-bed, and a Silicon Graphics IRIS Workstation. 
The arm pedestal is mounted on a mobile platform of the 
lathe-bed which provides one additional degree-of-freedom. The 
arm control unit has an electronic servo-level interface, 
which allows the user to communicate directly with the joint 
servomotors at a sampling frequency of f s =400 Hz, i.e., a 
sampling period of T s =2 . 5 ms. The joint servomotors can be 
commanded in any of the four modes: position, velocity, 
torque, and current. This makes it possible to operate the arm 
under either kinematic or dynamic control schemes, and 
therefore provides a testbed for validation of different 7 DOF 
control laws. In the present implementation, all seven joints 
are commanded in the position mode. 

The hardware diagram of the experimental setup is shown 
in Figure 11. The IRIS can operate in two different modes. 
First, it creates an interactive graphics simulation 
environment for analysis and control of the 7 DOF arm, as 
discussed in Section 3. Second, the IRIS serves as the 
graphical user interface through which the operator interacts 
with the actual arm in real-time and issues motion commands in 
joint or task space. Using this dual-mode functionality, the 
IRIS can be used initially in "preview mode" for animating the 
task scenario, and subsequently in "execution mode" to command 
the arm to duplicate the simulated motion. The software which 
provides graphical user interface and simulation capabilities 
resides on the IRIS. 

The VME-based real-time robot control system receives 
commands from the IRIS to move the actual arm. This is the 



part of the system which handles all real-time operations 
including computation of control laws and transmission of 
appropriate signals to the multibus-based arm control unit. 

The control unit dispatches the commands for execution to the 
seven joint motors of the arm to perform the task. The VME 
chassis configuration contains five CPU boards that 
communicate through a shared memory board to perform all the 
necessary computations to provide real-time manipulator 
control. The first CPU interfaces with the high-level software 
residing on the IRIS, receives commands from the operator and 
obtains acknowledgment and state information from the low 
level after command execution. This processor also serves as 
the master by scheduling the synchronous operations of the 
slave processors that perform the real-time computations. The 
second CPU performs real-time trajectory generation and 
kinematic computations. This includes generating the desired 
end-effector trajectories and computing the necessary 
kinematic and Jacobian transformations. The second CPU also 
accesses and updates the world model and performs computations 
to resolve the manipulator redundancy. The third CPU is 
designated to perform all the computations associated with 
invoking various dynamic control algorithms (not used at 
present) . The fourth CPU solely communicates with the arm 
control unit by executing the arm interface driver at every 
2.5 milliseconds. A two-card VME-to-multibus adaptor set from 
the BIT3 Corporation is employed to provide shared memory 
servo interface with the arm control unit at high speed. The 
role of the driver is to perform handshake with the arm 
control unit and to convert data into appropriate format for 
usage. Some of its features include translating data 
representation in the multibus to VMEbus format and vice versa 
and safety checking to avoid hitting physical joint limits and 
collision with the floor. The fifth CPU hosts various drivers 
that manipulate the shared memory board which contains global 
memory formation, read in joystick inputs, control the 
motorized lathe-bed, and interface with other devices such as 
a force/torque sensor and a gripper. All software executing on 



the VME environment is written in the "C" language. Code is 
developed on a SUN 3/60 UNIX computer utilizing SUN's "C" 
compiler and Wind River's VxWorks/Wind real-time library. The 
code is then downloaded through Ethernet to the target 
processor boards for immediate execution. 

To perform initial experiments, a computer program is 
written for trajectory generation, kinematic computations, and 
arm interface via the driver. At the present time, all of 
these computations are performed sequentially on one MC68020 
processor with a cycle period of 25 milliseconds. First, a 
simple cycloidal Cartesian-space trajectory is generated based 
on the operator's input of the desired arm goal configuration. 
The 7x1 arm configuration vector X includes the 6xi vector Y 
of position and orientation coordinates of the hand and the 
scalar arm angle i|i for redundancy resolution. At each 
computation cycle, the output from the trajectory generator is 
the 7x1 vector of Cartesian increments AX. The 7x7 augmented 
Jacobian J is also computed which embodies the redundancy 
resolution goal, namely i|» control in this case. The Jacobian 
is then inverted and multiplied by the Cartesian increments to 
generate the seven joint increments A©^* 1 A X. Finally, the 
joint setpoints are computed by adding the increments to the 
current joint angles and are dispatched to the arm interface 
driver to move the arm under position mode. 

In the present implementation, because of the slow 
sampling rate of 25 milliseconds, the Jacobian matrix J is 
computed using the desired joint angles instead of the actual 
joint angles. In addition, the Cartesian increment AX is 
calculated using the difference between the two consecutive 
desired Cartesian setpoints, not by subtracting the actual 
Cartesian values from the desired Cartesian setpoints. To 
improve performance, we plan to increase the servo rate by 
splitting the algorithm on two MC68020 processors. The first 
processor will be designated solely to communicate with the 
arm at every 2.5 milliseconds (running the driver as CPU 4). 
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The second processor will perform cycloidal trajectory 
generation and Jacobian computation and version. The first 
processor will then obtain the joint setpoints at every 25 
milliseconds, but will linearly interpolate these points into 
5 ten via-points which are then sent one at a time to the arm 

controller every 2.5 milliseconds. 
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In the experiment, the Robotics Research arm is initially 
at the predefined Cartesian "home" ("cstart") position with 
the end-effector coordinates (x, y, z, roll, pitch, yaw) and 
arm angle i|f as X(0) = [-900, 297, 316, 0°, 0°, 44°, 60°] measured 
relative to a fixed reference frame attached to the shoulder, 
where the lengths are millimeters and the angles are in 
degrees. This position corresponds to the joint angular values 
of 0(0)= [-63°, -61°, 78°, -88°, 79°, -85°, 159°] which is away 
from the arm singular configuration. Data are collected as all 
seven Cartesian coordinates move simultaneously from the 
"cstart" position to the user-specified goal position X(t)=, 
where the motion duration r is chosen as 10 seconds. This 
corresponds to the hand translational motion of 866 
millimeters. Preliminary experimental results which 
demonstrate trajectory tracking are presented in Figures 
12a — 12g. For each end-effector coordinate (x, y, z, roll, 
pitch, yaw) and the arm angle i|r, the tracking-error is 
computed by using the difference between the actual trajectory 
and the desired trajectory. Note that the maximum error occurs 
in the middle of the trajectory, i.e. at time t=r/2=5 seconds. 
This is because for a cycloidal position trajectory, the 
velocity is at its peak in the middle of the trajectory, which 
attributes to the maximum occurrence of linearization errors. 
From Figures 12a-g, in each positional coordinate, the maximum 
tracking-error does not exceed 16 millimeters, and in each 
orientational coordinate, the maximum error is less than 3 
degrees. Therefore, the experimental results demonstrate the 
efficacy of configuration control for the 7 DOF arm. Note that 
the tracking performance will be improved considerably when 
the computations are split on two processors so that the joint 
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setpoints are updated every 2.5 milliseconds. 


5. Conclusions 

The problem of motion control of 7 DOF arms is addressed 
in this paper. To provide dexterous motion of the arm, the 
configuration control approach is adopted in which the 
redundancy in joint space is effectively transferred into task 
space by adding a user-defined kinematic constraint to the 
end— effector task. The configuration control schemes are 
robust when singularities are encountered and allow the user 
to assign appropriate priorities to the task requirements. In 
this paper, applications of configuration control approach to 
motion control of the 7 DOF Robotics Research arm are 
described. Diverse redundancy resolution goals such as elbow 
control, collision avoidance and optimal joint movement are 
demonstrated using computer graphics simulations. A simple 
laboratory experiment on configuration control of the Robotics 
Research arm is described, and experimental results are 
presented. 

In contrast to Jacobian pseudoinverse methods which 
resolve the redundancy in joint space, the configuration 
control approach provides direct control of the manipulator in 
task space, where the task is performed. Furthermore, unlike 
pseudoinverse methods, the redundancy resolution goal is not 
restricted to optimization of a kinematic objective function. 
Finally, in contrast to pseudoinverse methods which do not 
ensure cyclicity of motion [22], the configuration control 
approach guarantees cyclic (i.e., conservative) motions of the 
manipulator, which is particularly important for repetitive 
tasks. By way of an example, in a 7 DOF arm under 
pseudoinverse control, the elbow is allowed to move without 
restraint during the hand motion, and the arm assumes 
^^ eren ^ configurations for a closed-path hand movement [23]; 
whereas under configuration control, both of these problems 
are circumvented. 
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Current work is focused on expanding the redundancy 
resolution goals, improving the computational efficiency, and 
performing further experiments on real-time motion control of 
the 7 DOF Robotics Research arm. 

5 

While the invention has been described in detail by 
specific reference to preferred embodiments thereof, it is 
understood that variations and modifications may be made 
without departing from the true spirit and scope of the 
10 invention. 



CONFIGURATION CONTROL OF SEVEN DEGREE OF FREEDOM ARMS 


ABSTRACT OF THE INVENTION 

A seven-degree-of-freedom robot arm with a six-degree-of- 
freedom end effector is controlled by a processor employing a 
6-by-7 Jacobian matrix for defining location and orientation 
of the end effector in terms of the rotation angles of the 
joints, a 1 (or more)-by-7 Jacobian matrix for defining 1 (or 
more) user-specified kinematic functions constraining location 
or movement of selected portions of the arm in terms of the 
joint angles, the processor combining the two Jacobian 
matrices to produce an augmented 7 (or more)-by-7 Jacobian 
matrix, the processor effecting control by computing in 
accordance with forward kinematics from the augmented 7-by-7 
Jacobian matrix and from the seven joint angles of the arm a 
set of seven desired joint angles for transmittal to the joint 
servo loops of the arms. One of the kinematic functions 
constrains the orientation of the elbow plane of the arm. 
Another one of the kinematic functions minimizing a sum of 
gravitational torques on the joints. Still another one of the 
kinematic functions constrains the location of the arm to 
perform collision avoidance. Generically, one of the 
kinematic functions minimizes a sum of selected mechanical 
parameters of at least some of the joints associated with 
weighting coefficients which may be changed during arm 
movement. The mechanical parameters may be velocity errors or 
position errors or gravity torques associated with individual 
joints . 
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